﻿#include<stdexcept>
#include<string>
#include"Curve.hpp"

namespace MMF { namespace BSpline {


    Curve::Curve()
        : mFindIdx(0)
        , mLastPos(0.0f)
    {

    }

    void Curve::SetControlPoints(const std::vector<TesVector3>& points)
    {
        //重新设置控制点需要重置状态
        Reset();
        mFragments.clear();

        //点数不够
        if (points.size() < 4)
            throw std::length_error((__FILE__ +  std::to_string(__LINE__)).c_str());

        //循环创建子线段
        size_t times = points.size() / 4;
        float curLength = 0.0;
        for (size_t i = 0; i < times; i++)  //最后一个需要小心处理
        {
            //获取点
            std::vector<TesVector3> pts(4);
            pts[0] = points[i * 3];
            pts[1] = points[i * 3 + 1];
            pts[2] = points[i * 3 + 2];
            pts[3] = points[i * 3 + 3];

            //生成小段
            Fragment frag(curLength, pts);

            //下一组增加长度
            curLength += frag.mLength;

            //添加片段
            mFragments.push_back(frag);
        }

        //如果有余数
        size_t num = points.size() % 4 + times;

        if (num == 2)
        {
            std::vector<TesVector3> pts(4);
            pts[0] = points[times * 3 - 2];
            pts[1] = points[times * 3 - 1];
            pts[2] = points[times * 3];
            pts[3] = points[times * 3 + 1];

            float l0 = sqrtf((pts[1].x - pts[0].x) * (pts[1].x - pts[0].x) + (pts[1].y - pts[0].y) * (pts[1].y - pts[0].y) + (pts[1].z - pts[0].z) * (pts[1].z - pts[0].z));
            float l1 = sqrtf((pts[2].x - pts[1].x) * (pts[2].x - pts[1].x) + (pts[2].y - pts[1].y) * (pts[2].y - pts[1].y) + (pts[2].z - pts[1].z) * (pts[2].z - pts[1].z));
            float l2 = sqrtf((pts[3].x - pts[2].x) * (pts[3].x - pts[2].x) + (pts[3].y - pts[2].y) * (pts[3].y - pts[2].y) + (pts[3].z - pts[2].z) * (pts[3].z - pts[2].z));

            Fragment frag(curLength - l0 - l1, pts);
            curLength += l2;
            mFragments.push_back(frag);
        }
        else if (num == 3)
        {
            std::vector<TesVector3> pts(4);
            pts[0] = points[times * 3 - 1];
            pts[1] = points[times * 3];
            pts[2] = points[times * 3 + 1];
            pts[3] = points[times * 3 + 2];

            float l0 = sqrtf((pts[1].x - pts[0].x) * (pts[1].x - pts[0].x) + (pts[1].y - pts[0].y) * (pts[1].y - pts[0].y) + (pts[1].z - pts[0].z) * (pts[1].z - pts[0].z));
            float l1 = sqrtf((pts[2].x - pts[1].x) * (pts[2].x - pts[1].x) + (pts[2].y - pts[1].y) * (pts[2].y - pts[1].y) + (pts[2].z - pts[1].z) * (pts[2].z - pts[1].z));
            float l2 = sqrtf((pts[3].x - pts[2].x) * (pts[3].x - pts[2].x) + (pts[3].y - pts[2].y) * (pts[3].y - pts[2].y) + (pts[3].z - pts[2].z) * (pts[3].z - pts[2].z));

            Fragment frag(curLength - l0, pts);
            curLength += (l1 + l2);
            mFragments.push_back(frag);
        }
        else if (num == 4)
        {
            std::vector<TesVector3> pts(4);
            pts[0] = points[times * 3];
            pts[1] = points[times * 3 + 1];
            pts[2] = points[times * 3 + 2];
            pts[3] = points[times * 3 + 3];
            Fragment frag(curLength, pts);
            curLength += frag.mLength;
            mFragments.push_back(frag);
        }
    }

    TesVector3 Curve::GetPoint(float distance)
    {
        //如果是正向找点
        if (distance >= mFragments[mFindIdx].mStartPos)
        {
            //向前找到正确的线段
            while(!mFragments[mFindIdx].isIn(distance))
                mFindIdx++;
        }
        else
        {
            //反向找点
            while(!mFragments[mFindIdx].isIn(distance))
                mFindIdx--;
        }

        //当前找点的位置
        mLastPos = distance;
        
        //获取插值
        return mFragments[mFindIdx].GetPoint(distance);
    }

    size_t Curve::FindIdx()
    {
        return mFindIdx;
    }

    bool Curve::IsEnd()
    {
        if (mFindIdx == mFragments.size() - 1 && mLastPos >= mFragments.back().mStartPos + mFragments.back().mLength)
            return true;

        return false;
    }

    float Curve::Length()
    {
        return mFragments.back().mStartPos + mFragments.back().mLength;
    }

    void Curve::Reset()
    {
        mFindIdx = 0;
        mLastPos = 0.0f;
    }
}}